import pybullet as p

import pyrosim.pyrosim as pyrosim


class LINK_SENSOR:
    
    
    def __init__(self, link_name):
        
        self.__link_name = link_name

        
    def get_value(self):
        
        return pyrosim.Get_Touch_Sensor_Value_For_Link(self.__link_name)
        

class ENV_SENSOR:
    
    
    def __init__(self, type, body_id) -> None:
        
        self.__type = type
        self.__body = body_id
        
    def get_value(self):
    
        if self.__type == "position":
            return p.getBasePositionAndOrientation(self.__body)[0]
        
        elif self.__type == "orientation":
            return p.getEulerFromQuaternion(p.getBasePositionAndOrientation(self.__body)[1])
